package org.opentcs.strategies.basic.dispatching.rerouting;

import jakarta.inject.Inject;
import org.opentcs.components.kernel.Router;
import org.opentcs.components.kernel.services.TCSObjectService;
import org.opentcs.data.TCSObjectReference;
import org.opentcs.data.model.Point;
import org.opentcs.data.model.Vehicle;
import org.opentcs.data.order.DriveOrder;
import org.opentcs.data.order.Route;
import org.opentcs.data.order.TransportOrder;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import java.util.List;
import java.util.Objects;
import java.util.Optional;
import java.util.Set;

import static java.util.Objects.requireNonNull;

public class DyReroutingStrategy extends AbstractReroutingStrategy
    implements ReroutingStrategy {
  private static final Logger LOG = LoggerFactory.getLogger(DyReroutingStrategy.class);
  private final VehiclePositionResolver vehiclePositionResolver;
  private final TCSObjectService objectService;
  private final Router router;

  @Inject
  public DyReroutingStrategy(Router router,
                             TCSObjectService objectService,
                             DyDriveOrderMerger driveOrderMerger,
                             VehiclePositionResolver vehiclePositionResolver) {
    super(router, objectService, driveOrderMerger);
    this.vehiclePositionResolver = requireNonNull(vehiclePositionResolver,
        "vehiclePositionResolver");
    this.objectService = requireNonNull(objectService,
        "objectService");
    this.router = requireNonNull(router,
        "router");
  }


  @Override
  public Optional<List<DriveOrder>> reroute(Vehicle vehicle) {
    if (!isVehicleAtExpectedPosition(vehicle)) {
      LOG.warn("Can't perform regular rerouting for {} located at unexpected position.",
          vehicle.getName());
      return Optional.empty();
    }

    TransportOrder currentTransportOrder = objectService.fetchObject(TransportOrder.class,
        vehicle.getTransportOrder());

    LOG.debug("{}: Determining the reroute source...", vehicle.getName());
    Optional<Point> optRerouteSource = determineRerouteSource(vehicle);
    if (optRerouteSource.isEmpty()) {
      LOG.warn("{}: Could not determine the reroute source. Not trying to reroute.",
          vehicle.getName());
      return Optional.empty();
    }
    Point rerouteSource = optRerouteSource.get();

    // Get all unfinished drive order of the transport order the vehicle is processing.
    List<DriveOrder> unfinishedOrders = getUnfinishedDriveOrders(currentTransportOrder);

    // Try to get a new route for the unfinished drive orders from the reroute source.
    Optional<List<DriveOrder>> optOrders = tryReroute(vehicle, unfinishedOrders, rerouteSource);
    return optOrders;
  }


  public Optional<List<DriveOrder>> getDriveOrders(Vehicle vehicle, List<DriveOrder> newDriveOrders, TransportOrder currentTransportOrder) {

    Optional<Point> optRerouteSource = determineRerouteSource(vehicle);
    if (optRerouteSource.isEmpty()) {
      LOG.warn("{}: Could not determine the reroute source. Not trying to reroute.",
          vehicle.getName());
    }
    Point rerouteSource = optRerouteSource.get();
    LOG.debug("Found a new route for {} from point {}: {}",
        vehicle.getName(),
        rerouteSource.getName(),
        newDriveOrders);
    restoreCurrentDriveOrderHistory(newDriveOrders, vehicle, currentTransportOrder, rerouteSource);

    return Optional.of(newDriveOrders);
  }

  private Optional<List<DriveOrder>> tryReroute(Vehicle vehicle,
                                                List<DriveOrder> driveOrders,
                                                Point sourcePoint) {
    LOG.debug("Trying to reroute drive orders for {} from {}. Current drive orders: {}",
        vehicle.getName(),
        sourcePoint,
        driveOrders);
    TransportOrder vehicleOrder = objectService.fetchObject(TransportOrder.class,
        vehicle.getTransportOrder());
    //使用自己编写的router进行路由计算
    return router.getDyRoute(
        vehicle,
        sourcePoint,
        new TransportOrder("reroute-dummy", driveOrders)
            .withProperties(vehicleOrder.getProperties())
    );
  }

  @Override
  protected Optional<Point> determineRerouteSource(Vehicle vehicle) {
    return Optional.of(vehiclePositionResolver.getFutureOrCurrentPosition(vehicle));
  }

  private boolean isVehicleAtExpectedPosition(Vehicle vehicle) {
    TransportOrder currentTransportOrder
        = getObjectService().fetchObject(TransportOrder.class, vehicle.getTransportOrder());
    TCSObjectReference<Point> currentVehiclePosition = vehicle.getCurrentPosition();
    DriveOrder currentDriveOrder = currentTransportOrder.getCurrentDriveOrder();
    if (currentVehiclePosition == null || currentDriveOrder == null) {
      return false;
    }

    int routeProgressIndex = currentTransportOrder.getCurrentRouteStepIndex();
    if (routeProgressIndex == TransportOrder.ROUTE_STEP_INDEX_DEFAULT) {
      Route.Step step = currentDriveOrder.getRoute().getSteps().get(0);
      Point expectedVehiclePosition
          = step.getSourcePoint() != null ? step.getSourcePoint() : step.getDestinationPoint();
      return Objects.equals(expectedVehiclePosition.getReference(), currentVehiclePosition);
    }

    Route.Step step = currentDriveOrder.getRoute().getSteps().get(routeProgressIndex);
    Point expectedVehiclePosition = step.getDestinationPoint();
    return Objects.equals(expectedVehiclePosition.getReference(), currentVehiclePosition);
  }

}
